/*
 * hal_single_abs_encoder.c
 *
 *  Created on: 2025 Apr 8
 *      Author: SFLY
 */
#include "hal_single_abs_encoder.h"


void Hal_Single_Encoder_Init(xt_hal_motor_end_single_abs_encoder_t *mencoder,xt_hal_reducer_end_single_abs_encoder_t *rencoder)
{
    mencoder->encoder_base_info.raw_value = 0;
    mencoder->encoder_base_info.encoderK =__builtin_dsp_fsdiv_qx320f(MATH_TWO_PI,  (float)(Motor_End_Encoder_Res));
    mencoder->encoder_base_info.invencoderK = (float)(Motor_End_Encoder_Res)*INV_MATH_TWO_PI;
    mencoder->encoder_pos = XT_MOTOR_END_ENCODER;
    mencoder->calib_value = 0;
    mencoder->calib_value_prev = 0;
    mencoder->elec_theta = 0.0f;
    mencoder->elec_offset = 0.0f;
    mencoder->mech_theta_offset = 0.0f;
    mencoder->multi_mech_theta = 0.0f;
    mencoder->abs_multi_mech_theta = 0.0f;
    mencoder->orderdir =0;
    mencoder->roundcnt = 0;
    for(int i=0;i<LUT_N;i++)
    {
        mencoder->offset_lut[i] = 0;
    }
    mencoder->encoder_read = BSP_Motor_Encoder_ReadAngle;
    mencoder->encoder_set_elec_offset = Hal_Motor_Encoder_SetElecOffset;
    mencoder->encoder_set_mech_offset = Hal_Motor_Encoder_SetMechOffset;
    mencoder->encoder_set_order = Hal_Motor_Encoder_SetOrderDir;
    mencoder->encoder_reset = Hal_Motor_Encoder_Reset;
    mencoder->encoder_update = Hal_Motor_Encoder_Update;
#if (ENCODER_STRUCT==0)
    rencoder->encoder_base_info.raw_value = 0;
#else
    rencoder->encoder_base_info.raw_value = 0;
    rencoder->encoder_base_info.encoderK =__builtin_dsp_fsdiv_qx320f(MATH_TWO_PI, (float)(Reducer_End_Encoder_Res));
    rencoder->encoder_base_info.invencoderK = (float)(Reducer_End_Encoder_Res)*INV_MATH_TWO_PI;
    rencoder->encoder_pos = XT_REDUCER_END_ENCODER;
    rencoder->calib_value = 0;
    rencoder->calib_value_prev = 0;
    rencoder->mech_theta_offset = 0.0f;
    rencoder->abs_multi_mech_theta = 0.0f;
    rencoder->multi_mech_theta = 0.0f;
    rencoder->roundcnt = 0;
    rencoder->orderdir = 0;
    rencoder->encoder_read = BSP_Reducer_Encoder_ReadAngle;
    rencoder->encoder_set_mech_offset = Hal_Reducer_Encoder_SetMechOffset;
    rencoder->encoder_set_order = Hal_Reducer_Encoder_SetOrderDir;
    rencoder->encoder_reset = Hal_Reducer_Encoder_Reset;
    rencoder->encoder_update = Hal_Reducer_Encoder_Update;
#endif
}

void Hal_Motor_Encoder_SetElecOffset(float elecoffset,void* encoder_void_ptr)
{
    xt_hal_motor_end_single_abs_encoder_t* encoder = (xt_hal_motor_end_single_abs_encoder_t*)encoder_void_ptr;
    encoder->elec_offset = elecoffset;
}

void Hal_Motor_Encoder_SetMechOffset(float mechoffset,void* encoder_void_ptr)
{
    xt_hal_motor_end_single_abs_encoder_t* encoder = (xt_hal_motor_end_single_abs_encoder_t*)encoder_void_ptr;
    encoder->mech_theta_offset = mechoffset;
}

void Hal_Motor_Encoder_SetOrderDir(int8_t orderdir,void* encoder_void_ptr)
{
    xt_hal_motor_end_single_abs_encoder_t* encoder = (xt_hal_motor_end_single_abs_encoder_t*)encoder_void_ptr;
      encoder->orderdir = orderdir;
}

void Hal_Motor_Encoder_Reset(void* encoder_void_ptr)
{
    xt_hal_motor_end_single_abs_encoder_t* encoder = (xt_hal_motor_end_single_abs_encoder_t*)encoder_void_ptr;
    encoder->encoder_base_info.raw_value = 0;
    encoder->encoder_base_info.encoderK =__builtin_dsp_fsdiv_qx320f(MATH_TWO_PI,  (float)(Motor_End_Encoder_Res));
    encoder->encoder_base_info.invencoderK = (float)(Motor_End_Encoder_Res)*INV_MATH_TWO_PI;
    encoder->calib_value = 0;
    encoder->calib_value_prev = 0;
    encoder->elec_theta = 0.0f;
    encoder->multi_mech_theta = 0.0f;
    encoder->abs_multi_mech_theta = 0.0f;
    encoder->roundcnt = 0;
}

void Hal_Motor_Encoder_Update(void* encoder_void_ptr)
{
    xt_hal_motor_end_single_abs_encoder_t* encoder = (xt_hal_motor_end_single_abs_encoder_t*)encoder_void_ptr;
    encoder->encoder_base_info.raw_value = encoder->encoder_read();//获取编码器原始值[0-Res]
    int off_1 =  encoder->offset_lut[encoder->encoder_base_info.raw_value>>7];
    int off_2 =  encoder->offset_lut[((encoder->encoder_base_info.raw_value>>7)+1)%LUT_N];
    int off_interp = off_1 + ((off_2 - off_1)*(encoder->encoder_base_info.raw_value- ((encoder->encoder_base_info.raw_value>>7)<<7))>>7);
    encoder->calib_value = encoder->encoder_base_info.raw_value+ off_interp;                                                           // Correct for nonlinearity with lookup table from calibration
    int temp_offset = encoder->orderdir*(encoder->calib_value - encoder->calib_value_prev);
    if(temp_offset> Motor_End_Encoder_Res/2){
        encoder->roundcnt -= 1;
         }
     else if (temp_offset < -Motor_End_Encoder_Res/2){
         encoder->roundcnt +=1;
         }
    encoder->calib_value_prev = encoder->calib_value;
    encoder->abs_multi_mech_theta = (MATH_TWO_PI * (float)(encoder->calib_value+Motor_End_Encoder_Res*encoder->roundcnt))/ (float)(Motor_End_Encoder_Res);
    encoder->multi_mech_theta = encoder->orderdir*(encoder->abs_multi_mech_theta - encoder->mech_theta_offset);//绝对位置换算
    //电气角度
    encoder->elec_theta = (0.0003835f * (float) ((MOTOR_PolePairs*encoder->calib_value)%Motor_End_Encoder_Res)) + encoder->elec_offset;
    if(encoder->elec_theta < 0) encoder->elec_theta += MATH_TWO_PI;
    else if(encoder->elec_theta > MATH_TWO_PI) encoder->elec_theta -= MATH_TWO_PI;
}

void Hal_Reducer_Encoder_SetMechOffset(float mechoffset,void* encoder_void_ptr)
{
    xt_hal_reducer_end_single_abs_encoder_t* encoder = (xt_hal_reducer_end_single_abs_encoder_t*)encoder_void_ptr;
    encoder->mech_theta_offset = mechoffset;
}

void Hal_Reducer_Encoder_SetOrderDir(int8_t orderdir,void* encoder_void_ptr)
{
    xt_hal_reducer_end_single_abs_encoder_t* encoder = (xt_hal_reducer_end_single_abs_encoder_t*)encoder_void_ptr;
    encoder->orderdir = orderdir;
}

void Hal_Reducer_Encoder_Reset(void* encoder_void_ptr)
{
    xt_hal_reducer_end_single_abs_encoder_t* encoder = (xt_hal_reducer_end_single_abs_encoder_t*)encoder_void_ptr;
    encoder->encoder_base_info.raw_value = 0;
    encoder->encoder_base_info.encoderK =__builtin_dsp_fsdiv_qx320f(MATH_TWO_PI,  (float)(Motor_End_Encoder_Res));
    encoder->encoder_base_info.invencoderK = (float)(Motor_End_Encoder_Res)*INV_MATH_TWO_PI;
    encoder->calib_value = 0;
    encoder->calib_value_prev = 0;
    encoder->multi_mech_theta = 0.0f;
    encoder->abs_multi_mech_theta = 0.0f;
    encoder->roundcnt = 0;
}

void Hal_Reducer_Encoder_Update(void* encoder_void_ptr)
{
    xt_hal_reducer_end_single_abs_encoder_t* encoder = (xt_hal_reducer_end_single_abs_encoder_t*)encoder_void_ptr;
    encoder->encoder_base_info.raw_value = encoder->encoder_read();//获取编码器原始值[0-Res]
    encoder->calib_value = encoder->encoder_base_info.raw_value;                                                           // Correct for nonlinearity with lookup table from calibration
    int temp_offset = encoder->calib_value - encoder->calib_value_prev;//encoder->orderdir*(encoder->calib_value - encoder->calib_value_prev);
    if(temp_offset> Motor_End_Encoder_Res/2){
        encoder->roundcnt -= 1;
         }
     else if (temp_offset < -Motor_End_Encoder_Res/2){
         encoder->roundcnt +=1;
         }
    encoder->calib_value_prev = encoder->calib_value;
    encoder->abs_multi_mech_theta = (MATH_TWO_PI * (float)(encoder->calib_value+Motor_End_Encoder_Res*encoder->roundcnt))/ (float)(Motor_End_Encoder_Res);
    encoder->multi_mech_theta = encoder->abs_multi_mech_theta - encoder->mech_theta_offset;//encoder->orderdir*(encoder->abs_multi_mech_theta - encoder->mech_theta_offset);//绝对位置换算
}


